#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  stopButton,     sensorTouch)
#pragma config(Sensor, I2C_1,  leftMotorEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port2,           frontRightMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           backRightMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           frontLeftMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port5,           backLeftMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port6,           armMotor,      tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!



/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

void stopMotors()
{
	motor[frontRightMotor] = 0;
	motor[frontLeftMotor]  = 0;
	motor[backRightMotor] = 0;
	motor[backLeftMotor]  = 0;
}

void moveForward(float tileDist){
	//Moves forward X tiles

	//1 tile = 24in
	//15.7 = 1 Rotation (inches traveled)
	//24 / 15.7 = 1.528 = Rotation per tile
	//1 rotation = 240.448 counts
	//367.563 = Counts per tile
	float countGoal = tileDist * 367.563;
	float currentCount = 0;
	//Motors at full speed
	motor[frontRightMotor] = 127;
	motor[frontLeftMotor]  = 127;
	motor[backRightMotor] = 127;
	motor[backLeftMotor]  = 127;

	nMotorEncoder[leftMotorEncoder] = 0; //Reset encoder count

	while(currentCount < countGoal){
		currentCount = nMotorEncoder[leftMotorEncoder];
	}

	stopMotors();
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	//Steps for starting on the blue tile in the hanging zone
	//1. Move Forward for ? ft
	moveForward(1); //TODO Get actual distance
	//2. Turn 90 degrees to the right
	//3. Move forward over bump and under barrier
	moveForward(3); //TODO Get actual distance
	//4. Stop

	//Steps for starting on the blue tile in the hanging zone
	//TODO



}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	int buttonpressed=SensorValue(stopButton);

	while (true)
	{
		//Remote control setting
		motor[frontRightMotor] = vexRT[Ch2];
		motor[backRightMotor]  = vexRT[Ch2]; //Left side of the robot is controlled by the left joystick, Y-axis
		motor[frontLeftMotor] = vexRT[Ch3];
		motor[backLeftMotor]  = vexRT[Ch3];
		motor[armMotor] = vexRT[Btn8U];
		buttonpressed=SensorValue(stopButton);
	}

	//if(vexRT[Btn5U] == 1)       	//If button 5U is pressed...
	{
		//motor[armMotor] = 127;    	//...raise the arm.
	}
	//else if(vexRT[Btn5D] == 1)  	//Else, if button 5D is pressed...
	{
		//motor[armMotor] = -127;   	//...lower the arm.
	}


	motor[frontRightMotor] = -10; //Tells all of the motors to go backwards a tiny bit, and waits 0.75 seconds.
	motor[backRightMotor]  = -10;
	motor[frontLeftMotor] = -10;
	motor[backLeftMotor]  = -10;
	motor[armMotor] = 0;
	wait1Msec(750);

	motor[frontRightMotor] = 0; //Tells all of the motors to stop, and waits 1 seconds.
	motor[backRightMotor]  = 0;
	motor[frontLeftMotor] = 0;
	motor[backLeftMotor]  = 0;
	motor[armMotor] = 0;

	wait1Msec(1000);

}
